#include <gtest/gtest.h>
#include <iostream>
#include <cstring>
#include <vector>
#include "ModbusClient.hpp"

class TestModbusClient : public ::testing::Test
{
protected:
    static void SetUpTestCase()
    {
    }

    static void TearDownTestCase()
    {
    }

    void SetUp() override
    {

        client_ = std::make_shared<ModbusClient>("192.168.3.90", 502);
        int ret = client_->initClient();
        if (ret != 1)
        {
            throw std::runtime_error("can not connect to plc");
        }
    }

    void TearDown() override
    {
        client_->close();
        client_->free();
    }

    ModbusClient::Ptr client_;
};

TEST_F(TestModbusClient, test_modbus_connection)
{
    bool isConnected = client_->isConnected();
    EXPECT_EQ(isConnected, true);
}

TEST_F(TestModbusClient, test_modbus_read)
{
    bool isConnected = client_->isConnected();
    EXPECT_EQ(isConnected, true);
    int num = 5;
    std::vector<uint16_t> value(num); // 使用 vector 自动管理内存

    bool isRead = client_->read(7645, num, value.data());
    EXPECT_EQ(isRead, true);

    for (size_t i = 0; i < num; i++)
    {
        std::cout << value[i] << std::endl;
    }
}

TEST_F(TestModbusClient, test_modbus_read_driver)
{
    bool isConnected = client_->isConnected();
    EXPECT_EQ(isConnected, true);
    int num = 8;
    std::vector<uint16_t> value(num); // 使用 vector 自动管理内存

    bool isRead = client_->read(7800, num, value.data());
    EXPECT_EQ(isRead, true);

    for (size_t i = 0; i < num; i = i + 2)
    {
        uint16_t high = value[i + 1]; // 高 16 位 (例如 100.0 的高位)
        uint16_t low = value[i];  // 低 16 位 (例如 100.0 的低位)

        // 将 high 和 low 重新组合为 32 位整数
        uint32_t intRepresentation = (static_cast<uint32_t>(high) << 16) | low;

        // 将 uint32_t 转换为 float
        float value;
        std::memcpy(&value, &intRepresentation, sizeof(value));
        std::cout << value << std::endl;
    }
}

TEST_F(TestModbusClient, test_modbus_write_right_light)
{
    bool isConnected = client_->isConnected();
    EXPECT_EQ(isConnected, true);

    // 右灯
    uint16_t value = 0;
    bool isWrite = client_->write(6103, value);
    EXPECT_EQ(isWrite, true);
}

TEST_F(TestModbusClient, test_modbus_write_speed)
{
    bool isConnected = client_->isConnected();
    EXPECT_EQ(isConnected, true);

    // 行走驱动 real  6206 角度 6208 速度 mm 6210 加速时间 6212 减速时间
    std::vector<float> msg = {0.0, 0.0, 10.0, 10.0};
    std::vector<uint16_t> data(msg.size() * 2); // 需要写入的寄存器值
    for (size_t i = 0; i < msg.size(); i++)
    {
        float value = msg[i];
        uint32_t intRepresentation;
        std::memcpy(&intRepresentation, &value, sizeof(value));
        uint16_t high = (intRepresentation >> 16) & 0xFFFF;
        uint16_t low = intRepresentation & 0xFFFF;
        // 小端字节序
        data[i * 2] = low;
        data[i * 2 + 1] = high;
    }

    bool isWrite = client_->writeBuff(6206, data.size(), data.data());
    EXPECT_EQ(isWrite, true);
}

TEST_F(TestModbusClient, test_modbus_write_fork)
{
    bool isConnected = client_->isConnected();
    EXPECT_EQ(isConnected, true);

    // 叉臂高度 real  6202 高度 mm 6204 速度
    std::vector<float> msg = {800.0, 95.0};
    std::vector<uint16_t> data(msg.size() * 2); // 需要写入的寄存器值
    for (size_t i = 0; i < msg.size(); i++)
    {
        float value = msg[i];
        uint32_t intRepresentation;
        std::memcpy(&intRepresentation, &value, sizeof(value));
        uint16_t high = (intRepresentation >> 16) & 0xFFFF;
        uint16_t low = intRepresentation & 0xFFFF;
        // 小端字节序
        data[i * 2] = low;
        data[i * 2 + 1] = high;
    }

    bool isWrite = client_->writeBuff(6202, data.size(), data.data());
    EXPECT_EQ(isWrite, true);
}